#include "gnss_imu_align/gnss_imu_align.h"
#include <fstream>
#include <ctime>
#include <iostream>
GNSS_IMU_Align::GNSS_IMU_Align(/* args */)
{
    auto p = eskf.getP();
    p.setIdentity();
    p(9,9) = p(10,10) = p(11,11) = 0.001;
    p(12,12) = p(13,13) = p(14,14) = 0.001;
    p(15,15) = p(16,16) = p(17,17) = 0.001; 
    eskf.setP(p);
    auto x = eskf.getX();
    auto q =  eskf.getQ();
    x.ba.setZero();
    x.bg.setZero();
    x.gravity.setZero();
    // x.gravity[2] = SlamCraft::GRAVITY;
    x.position.setZero();
    x.velocity.setZero();
    x.rotation.setIdentity();
        ///. Q
    cov_gyr = {0.1,0.1,0.1};
    cov_acc = cov_gyr;
    cov_bias_acc = {0.0001,0.0001,0.0001};
    cov_bias_gyr = cov_bias_acc;

    q.setZero();
    q.block<3, 3>(0, 0).diagonal() = cov_gyr;
    q.block<3, 3>(3, 3).diagonal() = cov_acc;
    q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
    q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
    eskf.setX(x);
    eskf.setQ(q);
    eskf.zhr_model = std::bind(&GNSS_IMU_Align::calc_eskf_zhr,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4);
    eskf.set_iter_times(1);
}

GNSS_IMU_Align::~GNSS_IMU_Align()
{
}
void GNSS_IMU_Align::process_group( MeasureGroupType&measure_group){
    //. 开始前向过程
    SlamCraft::IMU avg_imu;
    double dt;
    for (int i = -1; i < int(measure_group.imus_in.size()-1); i++)
    {

        if (i==-1)
        {
            avg_imu = (last_imu+measure_group.imus_in.front())*0.5;
            dt = measure_group.imus_in.front().stamp-last_imu.stamp;
        }else{
            avg_imu = (measure_group.imus_in[i]+measure_group.imus_in[i+1])*0.5;
            dt = measure_group.imus_in[i+1].stamp-measure_group.imus_in[i].stamp;
        }
        eskf.predict(avg_imu,dt);
    }
    dt = measure_group.end_gnss.stamp - measure_group.imus_in.back().stamp;
    eskf.predict(avg_imu,dt);
    last_imu = measure_group.imus_in.back();
    last_imu.stamp = measure_group.end_gnss.stamp;
    now_gnss = measure_group.end_gnss;
    // . ESKF update
    eskf.update_once();
}
bool GNSS_IMU_Align::init(const MeasureGroupType&measure_group){
    static bool imu_inited = false;
    static int imu_count = 0;
    if (imu_inited)
    {
        return imu_inited;
    }
    
    for (size_t i = 0; i < measure_group.imus_in.size(); i++)
    {
        imu_count++;
        auto x = eskf.getX();
        x.gravity += measure_group.imus_in[i].acc;
        x.bg += measure_group.imus_in[i].gro;
        x.rotation = measure_group.imus_in[i].rot;
        eskf.setX(x);

    }
    if (imu_count >= 5)
    {
        auto x = eskf.getX();
        x.gravity /=double(imu_count);

        x.bg /=double(imu_count);

        x.gravity =  -1*x.gravity ;
        eskf.setX(x);
        imu_inited = true;
        save_init_pose(measure_group.end_gnss);
    }
    last_imu = measure_group.imus_in.back();
    last_imu.stamp = measure_group.end_gnss.stamp;
    ez_geographic.reset_gnss_pose(measure_group.end_gnss);
    return imu_inited;
}
SlamCraft::ESKF::State_18 GNSS_IMU_Align::get_x(){
    return eskf.getX();
}
void GNSS_IMU_Align::calc_eskf_zhr(const SlamCraft::ESKF::State_18& X,Eigen::MatrixXd & Z,Eigen::MatrixXd & H,Eigen::MatrixXd & R){
    Eigen::Vector3d now_gnss_postion;
    ez_geographic.get_relative_position(now_gnss,now_gnss_postion);
    Z.resize(3,1);
    Z = now_gnss_postion-X.position;
    H.resize(3,18);
    H.setZero();
    H.block<3,3>(0,3).setIdentity();
}
Eigen::Vector3d GNSS_IMU_Align::debug_for_gnss_positon(const SlamCraft::GNSSDataType&gnss){
    Eigen::Vector3d gnss_postion;
    ez_geographic.get_relative_position(gnss,gnss_postion);
    return gnss_postion;
}
void GNSS_IMU_Align::save_init_pose(const SlamCraft::GNSSDataType&gnss){
    YAML::Node node;
    node["latitude"] = gnss.latitude;
    node["longitude"] = gnss.longitude;
    node["altitude"] = 0;
    std::ofstream fout(std::string(WORK_PROJ_DIR)+"/data/init_align_gnss.yaml",std::ios::out);
    fout << node;
    fout.close();
}